﻿using System;
using System.Linq;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace TileMapEditor
{
	public class PathFinding
	{
	    private Dictionary<Vector2, int> _map;
        private int _block_mark;

        private Graph _graph;

        private Vector2 _start;
        private Vector2 _end;

        public Action<Vector2,int> DrawVisitedCallback;

        private EditorCoroutine _bfs_coroutine;
        private EditorCoroutine _dijkstra_coroutine;
        private EditorCoroutine _astar_coroutine;

        private List<Vector2> _visited_pos;
        public List<Vector2> visitedPos
        {
            get
            {
                if ( null == _visited_pos )
                    _visited_pos = new List<Vector2>();

                return _visited_pos;
            }
        }

        public List<Vector2> _optimized_pos;
        public List<Vector2> optimizedPos
        {
            get
            {
                if ( null == _optimized_pos )
                    _optimized_pos = new List<Vector2>();

                return _optimized_pos;
            }
        }

        private List<Vector2> _found_path;
        public List<Vector2> foundPath
        {
            get
            {
                if ( null == _found_path )
                    _found_path = new List<Vector2>();

                return _found_path;
            }
        }

        private List<Vector2> _neaest_region;
        public List<Vector2> neaestRegion
        {
            get
            {
                if ( null == _neaest_region )
                    _neaest_region = new List<Vector2>();

                return _neaest_region;
            }
        }

        public PathFinding() { }

        public PathFinding(Dictionary<Vector2, int> map, int width, int height )
	    {
            _graph = new Graph( map, width, height );
	    }

        public void SetMap(Dictionary<Vector2, int> map, int width, int height )
        {
            _graph = new Graph( map, width, height );
        }

        /// <summary>
        /// TODO , the _end position doesn't in map, we will find the nearest position
        /// as the _end position.
        /// </summary>
        private void FindNearestPoint()
        {
            neaestRegion.Clear();

            if ( _start == _end )
                return;

            int max_radius = 63;
            for (int x = 1; x < max_radius; x++ )
            {
                for ( int i = -x; i <= x; i++ ) // radius.
                {
                    if (Mathf.Abs(i) == x ) // border. <left, right>
                    {
                        for ( int j = -x; j <= x;  j++ )
                        {
                            var pos = new Vector2( _end.x + i, _end.y + j ); // fixed.

                            if (_graph.IsPositionValid( pos ) )
                            {
                                //neaestRegion.Add( pos );
                                _end = pos;
                                return;
                            }
                        }
                    }
                    else // <top, bottom>
                    {
                        var p1 = new Vector2( _end.x + i, _end.y + x );

                        if ( _graph.IsPositionValid( p1 ) )
                        {
                            //neaestRegion.Add( p1 );
                            _end = p1;
                            return;
                        }

                        //var p2 = new Vector2( _end.x + i, _end.y - x );
                        p1 = new Vector2( _end.x + i, _end.y - x );
                        if ( _graph.IsPositionValid( p1 ) )
                        {
                            //neaestRegion.Add( p2 );
                            _end = p1;
                            return;
                        }
                    }
                }
            }
        }

        private void ReconstructPath( Dictionary<Vector2, Vector2> came_from )
        {
            var cur = _end;
            visitedPos.Clear();
            visitedPos.Add( cur );

            while ( cur != _start )
            {
                cur = came_from[cur];
                visitedPos.Add( cur );
            }

            visitedPos.Add( _start );
            visitedPos.Reverse();
        }

        private void OptimizedAstar()
        {

            if ( visitedPos.Count <= 1 )
                return;

            var polyNodes = new List<Vector2>();

            Graph.Direction prev_direction = Graph.Direction.NULL;
            Graph.Direction cur_direction = Graph.Direction.NULL;

            for ( int i = 0; i < visitedPos.Count; i++ )
            {
                var curpos = visitedPos[i];
                if ( i + 1 == visitedPos.Count )
                {
                    polyNodes.Add( curpos );
                    break;
                }
                else
                {
                    cur_direction = _graph.GetDirection( curpos, visitedPos[i + 1] );

                    if ( cur_direction != prev_direction )
                    {
                        polyNodes.Add( curpos );
                    }

                    prev_direction = cur_direction;
                }

            }

            optimizedPos.Clear();

            {
                foreach ( var polyNode in polyNodes )
                {
                    optimizedPos.Add( polyNode );
                }

                return;
            }
        }

        #region A-Star

        public void Astar( Vector2 start, Vector2 end )
        {
            visitedPos.Clear();

            _start = start;
            _end = end;

            if ( !_graph.IsPositionValid( _end ) )
            {
                FindNearestPoint();
            }

            _astar_coroutine = EditorCoroutine.start( AstarPrint() );
        }

        public void StopAstar()
        {
            if ( null != _astar_coroutine )
            {
                _astar_coroutine.stop();
                _astar_coroutine = null;
            }
        }

        IEnumerator AstarPrint()
        {
            var frontier = new PriorityQueue<float, Vector2>();
            frontier.Enqueue( _start, 0 );

            var cost_so_far = new Dictionary<Vector2, float>();
            cost_so_far.Add( _start, 0 );

            var came_from = new Dictionary<Vector2, Vector2>();
            came_from.Add( _start, _start );

            while ( frontier.Count != 0 )
            {
                var cur = frontier.Dequeue();

                if ( cur.Equals( _end ) )
                {
                    break;
                }

                var neighbours = _graph.GetNeighbours( cur );
                foreach ( var neighbour in neighbours )
                {
                    float new_cost = cost_so_far[cur] + _graph.Cost( cur, neighbour.Key, _end );
                    if ( ( !cost_so_far.ContainsKey( neighbour.Key ) ) || new_cost < cost_so_far[neighbour.Key] )
                    {
                        if ( cost_so_far.ContainsKey( neighbour.Key ) )
                            cost_so_far[neighbour.Key] = new_cost;
                        else
                            cost_so_far.Add( neighbour.Key, new_cost );

                        float priority = new_cost + _graph.Heuristic( cur, neighbour.Key, _end );
                        frontier.Enqueue( neighbour.Key, priority );

                        if ( came_from.ContainsKey( neighbour.Key ) )
                            came_from[neighbour.Key] = cur;
                        else
                            came_from.Add( neighbour.Key, cur );

                        //if ( null != DrawVisitedCallback)
                        //{
                        //    DrawVisitedCallback.Invoke( neighbour.Key, 0XFF );
                        //    yield return new WaitForSeconds( 0.001f );
                        //}
                    }
                }
            }

            ReconstructPath( came_from );

            OptimizedAstar();

            for ( int i = 0; i < visitedPos.Count; i++ )
            {
                DrawVisitedCallback( visitedPos[i], 0XFF );
                yield return new WaitForSeconds( 0.5f );
            }

            yield return null;
        }

        private bool CheckGoalPosition( Vector2 pos )
        {
            if ( _graph.IsPositionValid( pos ) )
                return true;

            return false;
        }


        #endregion End A-Star 

        internal class Graph
        {
            private Dictionary<Vector2, int> _map;
            private int _width;
            private int _height; 

            private Dictionary<Vector2, int> _neighbours;

            public Graph( Dictionary<Vector2, int> map, int width, int height )
            {
                _map = map;
                _width = width;
                _height = height;

                _neighbours = new Dictionary<Vector2, int>();
            }
            
            public Dictionary<Vector2, int> GetNeighbours( Vector2 pos )
            {
                _neighbours.Clear();

                for ( int i = -1; i <= 1; i++ )
                {
                    for ( int j = -1; j <= 1; j++ )
                    {
                        if ( i == 0 && j == 0 ) continue;

                        var neighbour = new Vector2( pos.x + i, pos.y + j);
                        if ( CheckRange( neighbour ) )
                            AddNeighbour( neighbour );
                    }
                }

                return _neighbours;
            }

            public Direction GetDirection( Vector2 from, Vector2 to )
            {
                if ( from.Equals( to ) )
                    return Direction.NULL;

                if ( Mathf.Abs( to.x - from.x ) < 1e-10 )
                {
                    if ( to.y > from.y )
                        return Direction.TOP;
                    else
                        return Direction.BOTTOM;
                }
                else if ( Mathf.Abs( to.y - from.y ) < 1e-10 )
                {
                    if ( to.x > from.x )
                        return Direction.LEFT;
                    else
                        return Direction.RIGHT;
                }
                else if ( to.x > from.x && to.y > from.y )
                {
                    return Direction.TOPLEFT;
                }
                else if ( to.x > from.x && to.y < from.y )
                {
                    return Direction.BOTTOMLEFT;
                }
                else if ( to.x < from.x && to.y > from.y )
                {
                    return Direction.TOPRIGHT;
                }
                else if ( to.x < from.x && to.y < from.y )
                {
                    return Direction.BOTTOMRIGHT;
                }

                return Direction.NULL;
            }


            public float Cost( Vector2 from, Vector2 to, Vector2 goal )
            {
                if ( from.x == to.x || from.y == to.y )
                    return 1f;

                return 1.414f;
            }

            public bool IsPositionValid( Vector2 pos )
            {
                if ( pos.x >= 0 && pos.x < _width
                    && pos.y >= 0 && pos.y < _height )
                {
                    // TODO: 0 is a markable number.
                    if ( _map.ContainsKey( pos ) && _map[pos] != 0 )
                    {
                        return true;
                    }
                }

                return false;
            }

            public bool IsPositionValid( float x, float y )
            {
                if ( x >= 0 && x < _width && y >= 0 && y < _height )
                {
                    // TODO: 0 is a markable number.
                    var pos = new Vector2( x, y );
                    if ( _map.ContainsKey( pos ) && _map[pos] != 0 )
                    {
                        return true;
                    }
                }

                return false;
            }

            public float Heuristic( Vector2 from, Vector2 to, Vector2 goal )
            {
                var dx = Mathf.Abs( goal.x - to.x );
                var dy = Mathf.Abs( goal.y - to.y );

                return ( dx + dy ) - 0.586f * Mathf.Min( dx, dy );
            }

            private void AddNeighbour( Vector2 pos )
            {
                if ( (! _neighbours.ContainsKey( pos )) && _map.ContainsKey( pos ) && _map[pos] != 0 )
                {
                    _neighbours.Add( pos, _map[pos] );
                }
            }

            private bool CheckRange( Vector2 pos )
            {
                if ( pos.x >= 0 && pos.x < _width 
                    && pos.y >= 0 && pos.y < _height )
                {
                    // TODO: 0 is a markable number.
                    if ( _map.ContainsKey ( pos ) && _map[pos] != 0 )
                    {
                        return true;
                    }
                }

                return false;
            }

            public enum Direction
            {
                NULL,
                TOP, TOPLEFT, LEFT, BOTTOMLEFT,
                BOTTOM, BOTTOMRIGHT, RIGHT, TOPRIGHT
            }
        }
	}
}
